#ifndef POST_PROCESSOR_H_
#define POST_PROCESSOR_H_
#include <boost/json.hpp>
#include <mutex>
#include <string>

#include "common/perception_defs.h"
#include "postprocess/common/common_var.h"
#include "postprocess/processor/pedestrian_processor.h"
#include "postprocess/processor/static_processor.h"
#include "postprocess/processor/vehicle_processor.h"

class PostProcessor final {
 public:
  /// @brief constuctor
  PostProcessor() noexcept;
  PostProcessor(PostProcessor const &) = delete;
  PostProcessor &operator=(PostProcessor const &) = delete;
  PostProcessor(PostProcessor &&) = delete;
  PostProcessor &operator=(PostProcessor &&) = delete;
  ~PostProcessor();

  ///@brief Get instance
  ///@return PostProcessor
  static PostProcessor &GetInstance() noexcept;

  ///@brief init
  ///@param settings PostProcessorSettings
  /// @param err error message
  /// @return if no error return true, otherwise false
  void Init() noexcept(false);

  /// @brief do post process
  /// @param perceptionResult perception result
  /// @param err error message
  /// @return if no error return true, otherwise false
  bool Process(autodrive::perception::PerceptionResult &perceptionResult,
               Eigen::Affine3d &pos, std::string &err) noexcept(false);

  /// @brief get current pos
  /// @return current pos
  Eigen::Affine3d GetCurPose() noexcept { return cur_pose_; }

  /// @brief get previous pos
  /// @return previous pos
  Eigen::Affine3d GetPrePose() noexcept { return pre_pose_; }

  /// @brief get current pos
  /// @return current pos
  void SetCurPose(Eigen::Affine3d pos) noexcept { cur_pose_ = pos; }

  /// @brief get previous pos
  /// @return previous pos
  void SetPrePose(Eigen::Affine3d pos) noexcept { pre_pose_ = pos; }

 private:
  /// @brief collect tracked results
  /// @param[OUT] lidar_objects: tracked objects with tracking information
  /// @return nothing
  void collect_tracked_results(
      autodrive::perception::PerceptionResult &perceptionResult,
      ObjectTrackSet &object_tracks_set) noexcept;

  /// @brief gen default bev fusion processor settings
  /// @return bev fusion processor settings
  PostProcessorSettings gen_default() noexcept;

  ///@brief init flag
  bool is_init_;
  ///@brief mutex , multi thread safe
  std::mutex mutex_;
  /// @brief vehicle processor
  VehicleProcessor vehicle_processor_;
  /// @brief pedestrian processor
  PedestrianProcessor pedestrian_processor_;
  /// @brief static barrier processor
  StaticProcessor static_processor_;
  /// @brief pre pose
  Eigen::Affine3d pre_pose_;
  /// @brief current pose
  Eigen::Affine3d cur_pose_;
  /// @brief post process settings
  PostProcessorSettings settings_;
};

namespace boost::json {
inline void tag_invoke(boost::json::value_from_tag, boost::json::value &jv,
                       PostProcessorSettings const &b) {
  jv = {
      {"track_cached_history_size_maximum",
       b.track_cached_history_size_maximum},
      {"track_acceleration_noise_maximum", b.track_acceleration_noise_maximum},
      {"track_speed_noise_maximum", b.track_speed_noise_maximum},
      {"collect_track_consecutive_invisible_maximum",
       b.collect_track_consecutive_invisible_maximum},
      {"collect_track_age_minimum", b.collect_track_age_minimum},
      {"initial_position_noise", b.initial_position_noise},
      {"initial_velocity_noise", b.initial_velocity_noise},
      {"initial_acceleration_noise", b.initial_acceleration_noise},
      {"initial_env_noiose", b.initial_env_noiose},
      {"vehicle_2diou_threshold", b.vehicle_2diou_threshold},
      {"vehicle_distance_threshold", b.vehicle_distance_threshold},
      {"pedestrian_2diou_threshold", b.pedestrian_2diou_threshold},
      {"pedestrian_distance_threshold", b.pedestrian_distance_threshold},
      {"use_kalman_breakdown", b.use_kalman_breakdown},
      {"vehicle_distance_breakdown_threshold",
       b.vehicle_distance_breakdown_threshold},
      {"vehicle_velocity_breakdown_threshold",
       b.vehicle_velocity_breakdown_threshold},
      {"vehicle_acceleration_breakdown_threshold",
       b.vehicle_acceleration_breakdown_threshold},
      {"pedestrian_distance_breakdown_threshold",
       b.pedestrian_distance_breakdown_threshold},
      {"pedestrian_velocity_breakdown_threshold",
       b.pedestrian_velocity_breakdown_threshold},
      {"pedestrian_acceleration_breakdown_threshold",
       b.pedestrian_acceleration_breakdown_threshold},
      {"motion_vehicle_Q", value_from(b.motion_vehicle_Q)},
      {"motion_vehicle_R", value_from(b.motion_vehicle_R)},
      {"motion_pedestrian_Q", value_from(b.motion_pedestrian_Q)},
      {"motion_pedestrian_R", value_from(b.motion_pedestrian_R)}};
}

inline PostProcessorSettings tag_invoke(
    boost::json::value_to_tag<PostProcessorSettings>,
    boost::json::value const &jv) {
  auto const &obj = jv.as_object();
  return PostProcessorSettings{
      boost::json::value_to<int>(obj.at("track_cached_history_size_maximum")),
      boost::json::value_to<float>(obj.at("track_acceleration_noise_maximum")),
      boost::json::value_to<float>(obj.at("track_speed_noise_maximum")),
      boost::json::value_to<int>(
          obj.at("collect_track_consecutive_invisible_maximum")),
      boost::json::value_to<int>(obj.at("collect_track_age_minimum")),
      boost::json::value_to<float>(obj.at("initial_position_noise")),
      boost::json::value_to<float>(obj.at("initial_velocity_noise")),
      boost::json::value_to<float>(obj.at("initial_acceleration_noise")),
      boost::json::value_to<float>(obj.at("initial_env_noiose")),
      boost::json::value_to<float>(obj.at("vehicle_2diou_threshold")),
      boost::json::value_to<float>(obj.at("vehicle_distance_threshold")),
      boost::json::value_to<float>(obj.at("pedestrian_2diou_threshold")),
      boost::json::value_to<float>(obj.at("pedestrian_distance_threshold")),
      boost::json::value_to<bool>(obj.at("use_kalman_breakdown")),
      boost::json::value_to<float>(
          obj.at("vehicle_distance_breakdown_threshold")),
      boost::json::value_to<float>(
          obj.at("vehicle_velocity_breakdown_threshold")),
      boost::json::value_to<float>(
          obj.at("vehicle_acceleration_breakdown_threshold")),
      boost::json::value_to<float>(
          obj.at("pedestrian_distance_breakdown_threshold")),
      boost::json::value_to<float>(
          obj.at("pedestrian_velocity_breakdown_threshold")),
      boost::json::value_to<float>(
          obj.at("pedestrian_acceleration_breakdown_threshold")),
      boost::json::value_to<std::vector<float>>(obj.at("motion_vehicle_Q")),
      boost::json::value_to<std::vector<float>>(obj.at("motion_vehicle_R")),
      boost::json::value_to<std::vector<float>>(obj.at("motion_pedestrian_Q")),
      boost::json::value_to<std::vector<float>>(obj.at("motion_pedestrian_R")),
  };
}

}  // namespace boost::json

#endif  // !POST_PROCESSOR_H_
